package ch.ethz.fcl.metrobuzz.render.techniques.cubic_hermite;

import javax.vecmath.Vector2d;

import ch.ethz.fcl.metrobuzz.spatialindex.object2d.Point2D;
import ch.ethz.fcl.metrobuzz.tools.MBTools;

/**
 * algorithm from http://en.wikipedia.org/wiki/Cubic_Hermite_spline
 * 
 * @author Zeng Wei
 * @email zeng@arch.ethz.ch
 */
public class CubicHermite {
	// Catmull Rom spline is a special case of cardinal spline with tension = 0
	public static Point2D[] getCatmullRomSpline(int pointSize,
			Point2D[] ctrlPoints) {
		return getCardinalSpline(pointSize, ctrlPoints, 0);
	}

	public static Vector2d getCatmullRomSplineTangent(int segmentPointSize,
			Point2D[] ctrlPoints, int pointIndex) {
		int segmentIndex = pointIndex / segmentPointSize;
		Point2D[] ps = new Point2D[4];

		if (segmentIndex == ctrlPoints.length - 1)
			segmentIndex = ctrlPoints.length - 2;
		
		ps[0] = null;
		if (segmentIndex == 0)
			ps[0] = ctrlPoints[0];
		else
			ps[0] = ctrlPoints[segmentIndex - 1];
		ps[1] = ctrlPoints[segmentIndex];
		ps[2] = ctrlPoints[segmentIndex + 1];
		ps[3] = null;
		if (segmentIndex + 2 == ctrlPoints.length)
			ps[3] = ctrlPoints[ctrlPoints.length - 1];
		else
			ps[3] = ctrlPoints[segmentIndex + 2];

		int pointInSegmentIndex = pointIndex % segmentPointSize;
		float t = pointInSegmentIndex / (float) segmentPointSize;

		Point2D pt = getPointAtT(ps, t, 0, 0, 0);

		float ta = t + 0.0001f;
		Point2D pta = getPointAtT(ps, ta, 0, 0, 0);

		return new Vector2d(pta.getX() - pt.getX(), pta.getY() - pt.getY());
	}

	/**
	 * get spline with additional control points defined as the up mid of two
	 * neighboring original points
	 * 
	 * @param pointSize
	 *            points size between two control points
	 * @param ctrlPoints
	 * @param withAdditional
	 *            if additional control points needed
	 * @return
	 */
	public static Point2D[] getCatmullRomSpline(int pointSize,
			Point2D[] ctrlPoints, boolean withAdditional) {
		if (ctrlPoints.length < 2)
			return null;

		if (!withAdditional)
			return getCatmullRomSpline(pointSize, ctrlPoints);

		else {
			Point2D[] newCtrlPoints = new Point2D[2 * ctrlPoints.length - 1];
			for (int i = 0; i < ctrlPoints.length - 1; i++) {
				Point2D p1 = ctrlPoints[i];
				Point2D p2 = ctrlPoints[i + 1];
				boolean up = i % 2 == 0 ? true : false;
				Vector2d dir = new Vector2d(p2.getX() - p1.getX(), p2.getY()
						- p1.getY());
				double upDirAngle = Math.PI / 2;
				if (!up)
					upDirAngle = -Math.PI / 2;
				Vector2d verticalDir = MBTools.rotate(dir, upDirAngle);
				float length = p1.distance(p2);
				Point2D midPt = p1.getPointAt(p2, 0.5f);
				Point2D midCtrlPt = midPt.getPointAt(verticalDir, length / 2);

				newCtrlPoints[i * 2] = p1;
				newCtrlPoints[i * 2 + 1] = midCtrlPt;
			}

			newCtrlPoints[newCtrlPoints.length - 1] = ctrlPoints[ctrlPoints.length - 1];

			return getCatmullRomSpline(pointSize, newCtrlPoints);
		}
	}

	// Cardinal spline with is Kochanek Bartels spline wiht bias = 0 and
	// continuity = 0
	public static Point2D[] getCardinalSpline(int pointSize,
			Point2D[] ctrlPoints, float tension) {
		return getKochanekBartelsSpline(pointSize, ctrlPoints, tension, 0, 0);
	}

	// http://en.wikipedia.org/wiki/Kochanek-Bartels_spline
	public static Point2D[] getKochanekBartelsSpline(int segmentPointSize,
			Point2D[] ctrlPoints, float tension, float bias, float continuity) {
		Point2D[] points = new Point2D[(ctrlPoints.length - 1)
				* segmentPointSize];

		for (int i = -1; i < ctrlPoints.length - 2; i++) {
			Point2D[] ps = new Point2D[4];
			ps[0] = null;
			if (i == -1)
				ps[0] = ctrlPoints[0];
			else
				ps[0] = ctrlPoints[i];
			ps[1] = ctrlPoints[i + 1];
			ps[2] = ctrlPoints[i + 2];
			ps[3] = null;
			if (i + 3 == ctrlPoints.length)
				ps[3] = ctrlPoints[ctrlPoints.length - 1];
			else
				ps[3] = ctrlPoints[i + 3];

			for (int j = 0; j < segmentPointSize; j++) {
				float t = j / (float) segmentPointSize;

				points[(i + 1) * segmentPointSize + j] = getPointAtT(ps, t,
						tension, bias, continuity);
			}
		}

		return points;
	}

	private static Point2D getPointAtT(Point2D[] ctrlPts, float t,
			float tension, float bias, float continuity) {
		if (ctrlPts.length != 4)
			return null;
		Point2D p0 = ctrlPts[0], p1 = ctrlPts[1], p2 = ctrlPts[2], p3 = ctrlPts[3];

		float a = 2 * t * t * t - 3 * t * t + 1;
		float b = t * t * t - 2 * t * t + t;
		float c = -2 * t * t * t + 3 * t * t;
		float d = t * t * t - t * t;

		float m1x = (1 - tension) * (1 + bias) * (1 + continuity)
				* (p1.getX() - p0.getX()) / 2 + (1 - tension) * (1 - bias)
				* (1 - continuity) * (p2.getX() - p1.getX()) / 2;
		float m1y = (1 - tension) * (1 + bias) * (1 + continuity)
				* (p1.getY() - p0.getY()) / 2 + (1 - tension) * (1 - bias)
				* (1 - continuity) * (p2.getY() - p1.getY()) / 2;

		float m2x = (1 - tension) * (1 + bias) * (1 - continuity)
				* (p2.getX() - p1.getX()) / 2 + (1 - tension) * (1 - bias)
				* (1 + continuity) * (p3.getX() - p2.getX()) / 2;
		float m2y = (1 - tension) * (1 + bias) * (1 - continuity)
				* (p2.getY() - p1.getY()) / 2 + (1 - tension) * (1 - bias)
				* (1 + continuity) * (p3.getY() - p2.getY()) / 2;

		float x = a * p1.getX() + b * m1x + c * p2.getX() + d * m2x;
		float y = a * p1.getY() + b * m1y + c * p2.getY() + d * m2y;

		return new Point2D(x, y);
	}
}